using System;
using System.Collections.Generic;
using System.Text;
using DotNetMatrix;
using ScannerLib.Vector;

namespace ScannerLib.PlaneCalibration
{
    public static class PlaneCalibrator
    {
        public static DeskCalibrationInfo Calibrate(BasicVector[] ppdPoints)
        {
            if (ppdPoints.Length != 4)
            {
                throw new ApplicationException("Calibration is performed using 4 points of a square");
            }

            BasicVector l1p, l2p, l3p, l4p, l5p, l6p; // square side representative lines in pixel coords
            BasicVector v1p, v2p, v3p, v4p; // vanishing points of l1/l2, l3/l4, l5p and l6p. we use l1/l2 and l3/l4 to establish horizon line.v3 and v4 are used for focus
            BasicVector lhp,lh,focus; // horizon in pixel coords and homogenious
            double cameraHeight = 0;

            l1p = BasicVector.CrossProduct(ppdPoints[0], ppdPoints[1]);
            l2p = BasicVector.CrossProduct(ppdPoints[2], ppdPoints[3]);
            l3p = BasicVector.CrossProduct(ppdPoints[1], ppdPoints[2]);
            l4p = BasicVector.CrossProduct(ppdPoints[3], ppdPoints[0]);
            l5p = BasicVector.CrossProduct(ppdPoints[0], ppdPoints[2]);
            l6p = BasicVector.CrossProduct(ppdPoints[1],ppdPoints[3]);

            //l1p = BasicVector.Scale(l1p, -1 / l1p.Z);
            //l2p = BasicVector.Scale(l2p, -1 / l2p.Z);
            //l3p = BasicVector.Scale(l3p, -1 / l3p.Z);
            //l4p = BasicVector.Scale(l4p, -1 / l4p.Z);
            //l5p = BasicVector.Scale(l5p, -1 / l5p.Z);
            //l6p = BasicVector.Scale(l6p, -1 / l6p.Z);

            v1p = BasicVector.CrossProduct(l1p, l2p);
            v2p = BasicVector.CrossProduct(l3p, l4p);

            v1p = BasicVector.Scale(v1p, 1 / v1p.Z);
            v2p = BasicVector.Scale(v2p, 1 / v2p.Z);

            lhp = BasicVector.CrossProduct(v1p, v2p);
            //lhp = BasicVector.Scale(lhp, -1/lhp.Z);

            v3p = BasicVector.CrossProduct(l5p, lhp);
            v4p = BasicVector.CrossProduct(l6p, lhp);

            v3p = BasicVector.Scale(v3p, 1 / v3p.Z);
            v4p = BasicVector.Scale(v4p, 1 / v4p.Z);

            try 
            {
                double fx = (v1p.X * v2p.X * v3p.Y * v4p.Y - v3p.X * v4p.X * v1p.Y * v2p.Y)
                    / (v1p.Y * v2p.Y  - v3p.Y * v4p.Y );

                double fy=0;

                if (fx <= 0)
                {
                    GeneralMatrix gm = new GeneralMatrix(2, 2, 0);

                    gm.SetElement(0, 0, v1p.X * v2p.X);
                    gm.SetElement(0, 1, v1p.Y * v2p.Y);
                    gm.SetElement(1, 0, v3p.X * v4p.X);
                    gm.SetElement(1, 1, v3p.Y * v4p.Y);

                    int rank = gm.Rank();

                    if (rank == 1)
                    {

                        fx = gm.GetElement(0, 0) + gm.GetElement(0, 1) + gm.GetElement(1, 0) + gm.GetElement(1, 1);

                        fy = fx = -fx / 2;
                    }
                }
                else
                {

                    fy = (v1p.X * v2p.X * v3p.Y * v4p.Y - v3p.X * v4p.X * v1p.Y * v2p.Y)
                       / (v3p.X * v4p.X  - v1p.X * v2p.X );
                }

                //if (fy <= 0)
                //    return null;

                fx = Math.Sqrt(fx);
                fy = Math.Sqrt(fy);

                focus = new BasicVector(1/fx,1/fy,1);
                lh = BasicVector.MultDiag(lhp,focus);
                cameraHeight = lh.Length;
                BasicVector tlh = BasicVector.Scale(lh, 1 / (lh.Length * lh.Length ));

                BasicVector[] realPoints = new BasicVector[4];

                for( int i=0; i<4; i++)
                {
                    BasicVector adjustedPoint = BasicVector.MultDiag(ppdPoints[i], focus);
                    realPoints[i] = BasicVector.Scale(adjustedPoint, 1 / BasicVector.DotProduct(adjustedPoint, tlh));
                }

                double space = BasicVector.Sum(realPoints[0], BasicVector.Scale(realPoints[1], -1)).Length * BasicVector.Sum(realPoints[1], BasicVector.Scale(realPoints[2], -1)).Length;
                double sidelen = Math.Sqrt(space);
                double ratio = 124 / sidelen;

                lh = BasicVector.Scale(lh, ratio);
                cameraHeight = lh.Length;
                lh = BasicVector.Scale(lh, 1 / cameraHeight);

                DeskCalibrationInfo dci = new DeskCalibrationInfo(focus, lh, cameraHeight);
                return dci;
            } catch 
            {
                return null; // can not be recovered
            }

            
        }
    }
}


